/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: HMC6352 Compass Driver
 *         $Id: compass.c 43 2014-04-21 17:08:28Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <string.h>

// Maxim CMSIS includes
#include "config.h"
#include "errors.h"
#include "i2c.h"

// RTOS includes
#include "FreeRTOS.h"
#include "task.h"

#include "compass.h"

/***** Definitions *****/
#define COMPASS_ADDR    (0x42 >> 1)

#define COMPASS_BUFFER_SIZE   4
#define COMPASS_BUFFER_SHIFT  2

/***** Global Data *****/
int compass_heading;
int compass_buffer[COMPASS_BUFFER_SIZE];

/***** Function Prototypes *****/
static void compass_task(void * pvParameters);

/****************************************************************************/
int eeprom_read(unsigned int addr, uint8_t * data)
{
  int err;
  uint8_t msg[2];
  volatile uint32_t delay;

  msg[0] = 'r';
  msg[1] = addr;
  if ((err = i2c_write((void*)MXC_BASE_I2C, COMPASS_ADDR, msg, 2)) != E_NO_ERROR) {
    return err;
  }

  for (delay = 0; delay < 0xFFF; delay++);

  if ((err = i2c_read((void*)MXC_BASE_I2C, COMPASS_ADDR, msg, 1)) != E_NO_ERROR) {
    return err;
  }

  *data = msg[0];

  return E_NO_ERROR;
}

/****************************************************************************/
int eeprom_write(unsigned int addr, uint8_t data)
{
  int err;
  uint8_t msg[3];
  volatile uint32_t delay;

  msg[0] = 'w';
  msg[1] = addr;
  msg[2] = data;
  if ((err = i2c_write((void*)MXC_BASE_I2C, COMPASS_ADDR, msg, 3)) != E_NO_ERROR) {
    return err;
  }

  for (delay = 0; delay < 0xFFF; delay++);

  return E_NO_ERROR;
}

/****************************************************************************/
int ram_read(unsigned int addr, uint8_t * data)
{
  int err;
  uint8_t msg[2];
  volatile uint32_t delay;

  msg[0] = 'g';
  msg[1] = addr;
  if ((err = i2c_write((void*)MXC_BASE_I2C, COMPASS_ADDR, msg, 2)) != E_NO_ERROR) {
    return err;
  }

  for (delay = 0; delay < 0xFFF; delay++);

  if ((err = i2c_read((void*)MXC_BASE_I2C, COMPASS_ADDR, msg, 1)) != E_NO_ERROR) {
    return err;
  }

  *data = msg[0];

  return E_NO_ERROR;
}

/****************************************************************************/
int opbyte_write(uint8_t data)
{
  int err;
  uint8_t msg[3];
  volatile uint32_t delay;

  msg[0] = 'G';
  msg[1] = 0x74;
  msg[2] = data;
  if ((err = i2c_write((void*)MXC_BASE_I2C, COMPASS_ADDR, msg, 3)) != E_NO_ERROR) {
    return err;
  }

  for (delay = 0; delay < 0xFFF; delay++);

  return E_NO_ERROR;
}

/****************************************************************************/
int compass_init(void)
{
  int err;
  uint8_t data;

  compass_heading = -1;

  if (eeprom_read(0, &data) != E_NO_ERROR)
    return 0;

  data = 'W';
  if ((err = i2c_write((void*)MXC_BASE_I2C, COMPASS_ADDR, &data, 1)) != E_NO_ERROR) {
    return err;
  }

  if ((err = eeprom_write(6, 16)) != E_NO_ERROR) {
    return err;
  }

  // Set compass to 20Hz, continuous
  if ((err = opbyte_write(0x72)) != E_NO_ERROR) {
    return err;
  }

  data = 'A';
  if ((err = i2c_write((void*)MXC_BASE_I2C, COMPASS_ADDR, &data, 1)) != E_NO_ERROR) {
    return err;
  }

  // Create the task
  if (xTaskCreate(compass_task, (const char * const)"compass", 200, NULL, 1, NULL) != pdPASS) {
    return E_NONE_AVAIL;
  }

  return E_NO_ERROR;
}

/****************************************************************************/
void compass_task(void * pvParameters)
{
  uint8_t msg[2];
  int newval;

  TickType_t xLastWakeTime;
  const TickType_t xFrequency = 50 / portTICK_PERIOD_MS;

  // Initialise the xLastWakeTime variable with the current time.
  xLastWakeTime = xTaskGetTickCount();

  while(1) {
    // Wait for the next cycle.
    vTaskDelayUntil(&xLastWakeTime, xFrequency);

    if (i2c_read((void*)MXC_BASE_I2C, COMPASS_ADDR, msg, 2) != E_NO_ERROR) {
      compass_heading = -1;
    } else {
      newval = msg[0];
      newval <<= 8;
      newval += msg[1];
      compass_heading = newval;
    }
  }
}

/****************************************************************************/
int compass_cal_start(void)
{
  int err;
  uint8_t data;

  data = 'C';
  if ((err = i2c_write((void*)MXC_BASE_I2C, COMPASS_ADDR, &data, 1)) != E_NO_ERROR) {
    return err;
  }

  return E_NO_ERROR;
}

/****************************************************************************/
int compass_cal_end(void)
{
  int err;
  uint8_t data;

  data = 'E';
  if ((err = i2c_write((void*)MXC_BASE_I2C, COMPASS_ADDR, &data, 1)) != E_NO_ERROR) {
    return err;
  }

  data = 'A';
  if ((err = i2c_write((void*)MXC_BASE_I2C, COMPASS_ADDR, &data, 1)) != E_NO_ERROR) {
    return err;
  }

  return E_NO_ERROR;
}

/****************************************************************************/
// Convert degrees*10 to rad*2^8
int deg10_to_rads8(int deg10)
{
  return ( (((int32_t)deg10 * 3217) + 3600) / 7200 );
}


/****************************************************************************/
// Convert degrees*10 to rad*2^8
int rads8_to_deg(int rads8)
{
  return ( (((int32_t)rads8 * 720) + 1609) / 3217 );
}
